#pragma once
#include "math_.h"
#include "data_structure.h"
/**
 * 初始化三维卡尔曼滤波器（匀速度模型）
 * dt：采样时间(s)，process_noise：过程噪声系数，measure_noise：测量噪声系数
 */
void kalman3d_init(KalmanFilter3D *kf, double dt, double process_noise, double measure_noise);

/**
 * 卡尔曼滤波时间更新（预测）
 */
void kalman3d_predict(KalmanFilter3D *kf);

/**
 * 卡尔曼滤波量测更新（校正）
 * z_x, z_y, z_z：测量值（直角坐标）
 */
void kalman3d_update(KalmanFilter3D *kf, double z_x, double z_y, double z_z);
